#include "DensityPeakAlgorithm.h"
#include "../Math/MathTools.h"
#include <fstream>
#include "../Tools/ZLogManager.h"
#include <numeric>

GLOBAL_NAMESPACE_BEGIN

namespace MLAlgorithms
{
    struct SortData
    {
        double data;
        int index;
    };

    static inline bool compare(const SortData& lhs, const SortData& rhs)
    {
        return lhs.data > rhs.data;
    }

    static double distance_l2(const Eigen::VectorXd& p, const Eigen::VectorXd& q)
    {
        return (p - q).squaredNorm();
    }

    DensityPeak::DensityPeak()
        : pAnn_(NULL)
        , pAnnRho_(NULL)
    {
        param_.dcScale = 0.3;
    }

    DensityPeak::~DensityPeak()
    {
        destroy();
    }

    void DensityPeak::destroy()
    {
        if (pAnn_)
            delete pAnn_;
        pAnn_ = NULL;

        if (pAnnRho_)
            delete pAnnRho_;
        pAnnRho_ = NULL;
    }

    bool DensityPeak::readFromFile(const char* fileName)
    {
        std::ifstream fin(fileName);
        if (!fin)
            return false;

        int nSize, nDim;

        fin >> nSize >> nDim;

        // currently, only accept dimension = 2
        assert(nDim == 2);

        // NOT Sure whether it works for size changed
        Eigen::MatrixXd data(nSize, nDim);
        for (int i = 0; i < nSize; i++)
        {
            fin >> data(i, 0) >> data(i, 1);
        }

        init(data);

        fin.close();

        return true;
    }

    bool DensityPeak::saveResultToFile(const char* fileName)
    {
        int nSize = getPointSize();
        if (rhos_.size() != nSize || deltas_.size() != nSize || clusterIds_.size() != nSize)
            return false;

        std::ofstream fout(fileName);
        if (!fout)
            return false;

        fout << nSize << " " << param_.dcScale << " " << param_.maxDist << "\n";
        for (int i = 0; i < nSize; i++)
        {
            fout << rhos_[i] << " "
                << deltas_[i] << " "
                << rhoTimesDeltas_[i] << " "
                << clusterIds_[i] << " "
                << "\n";
        }

        fout.close();

        return true;
    }

    bool DensityPeak::readResultFromFile(const char* fileName)
    {

        std::ifstream fin(fileName);
        if (!fin)
            return false;
        int nSize = 0;
        double tmp;
        fin >> nSize >> tmp >> tmp;  

        rhos_.resize(nSize, 0);
        deltas_.resize(nSize, 0);
        clusterIds_.resize(nSize, -1);

        for (int i = 0; i < nSize; i++)
        {
            fin >> rhos_[i] >> deltas_[i] >> tmp >> clusterIds_[i];
        }

        fin.close();

        computeTimes();

        return true;
    }

    int DensityPeak::getPointSize()
    {
        if (data_.rows() != 0)
            return data_.rows();
        else if (rhos_.size() != 0 && rhos_.size() == deltas_.size())
            return rhos_.size();
        return 0;
    }

    int DensityPeak::getPointDim()
    {
        return data_.cols();
    }

    void DensityPeak::init(const Eigen::MatrixXd& data)
    {
        data_ = data;
        if (pAnn_) delete pAnn_;
        pAnn_ = new FLAnnWarper<MATH::L2_Distance>(1);
        pAnn_->init(data);
    }

    void DensityPeak::setData(const Eigen::MatrixXd& data)
    {
        data_ = data;
    }

    bool DensityPeak::commit()
    {
        if (data_.cols() == 0 || data_.rows()==0)
            return false;
        computeParams();
        computeRho();
        computeDelta();
        computeTimes();
        // prepare the vectors
        clusterIds_.resize(getPointSize(), -1);
        return true;
    }

    void DensityPeak::computeParams()
    {
        computeMaxDist();
        param_.dc = param_.maxDist * param_.dcScale;
        DebugLog << " ..Parameter dc: " << param_.dc;
    }

    void DensityPeak::computeParams_old()
    {
        // compute d_c
        //double minDist = DBL_MAX;
        int nSize = getPointSize();
#if 1
        std::vector< std::vector<int> > nnIdx;
        std::vector< std::vector<double> > nnDists;
        pAnn_->setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
        pAnn_->knnSearch(data_, nnIdx, nnDists, 2);
        std::vector<double> distances(nSize, 0);
        for (size_t i = 0; i < nSize; i++)
        {
            double dist = distance_l2(data_.row(i), data_.row(nnIdx[i][1]));
            //DebugLog << data_.row(i);
            //DebugLog << data_.row(nnIdx[i][1]);
            distances[i] = nnDists[i][1];
        }
#endif

#if 0
        std::vector<int> nnIdx;
        std::vector<double> nnDists;
        pAnn_->setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
        std::vector<double> distances(nSize, 0);
        for (size_t i = 0; i < nSize; i++)
        {
            nnIdx.clear();
            nnDists.clear();
            //DebugLog << data_.row(i);
            pAnn_->knnSearch(data_.row(i), nnIdx, nnDists, 2);
            double dist = distance_l2(data_.row(i), data_.row(nnIdx[1]));
            //DebugLog << data_.row(nnIdx[1]);
            distances[i] = dist;
        }
#endif
        //double minDist = *std::min_element(distances.begin(), distances.end());
        double sumDist = std::accumulate(distances.begin(), distances.end(), 0.0);
        param_.dc = param_.dcScale * sumDist/distances.size();
        DebugLog << " ..Parameter dc: " << param_.dc;
        //param.dc *= param.dc;
        computeMaxDist();
    }

    void DensityPeak::computeMaxDist()
    {
        // using sampling method to estimate the max distance

        int nSize = getPointSize();
        int nSample = 0;
        if (nSize < 500)
            nSample = nSize;
        nSample = (int)(nSize*0.01);
        if (nSample > 500)
            nSample = 500;

        int curId = 0;
        double maxDist = 0;
        for (size_t i = 0; i < nSample; i++)
        {
            Eigen::VectorXd curV(data_.row(curId));
            for (size_t j = 0; j < nSize; j++)
            {
                double dist = distance_l2(curV, data_.row(j));
                if (dist > maxDist)
                {
                    maxDist = dist;
                    curId = j;
                }
            }
        }
        param_.maxDist = maxDist;

        DebugLog << " ..Maximun distance: " << param_.maxDist;
    }

    void DensityPeak::computeRho()
    {
        int nSize = getPointSize();
#if 1
        std::vector< std::vector<int> > nnIdx;
        std::vector< std::vector<double> > nnDists;
        pAnn_->setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
        pAnn_->radiusSearch(data_, nnIdx, nnDists, param_.dc);
        rhos_.resize(nSize, 0);
        for (size_t i = 0; i < nSize; i++)
        {
            rhos_[i] = nnIdx[i].size();
        }
#endif

#if 0
        std::vector<int> nnIdx;
        std::vector<double> nnDists;
        rhos_.resize(nSize, 0);
        pAnn_->setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
        for (size_t i = 0; i < nSize; i++)
        {
            pAnn_->radiusSearch(data_.row(i), nnIdx, nnDists, param_.dc);
            rhos_[i] = nnIdx.size();
        }
#endif
        DebugLog << " ..finish computing rhos.";
    }

    void DensityPeak::computeDelta()
    {
        // build inverse rho ann warper
        int nSize = getPointSize();
        Eigen::MatrixXd inverseRho(nSize, 1);
        for (size_t i = 0; i < nSize; i++)
        {
            inverseRho(i, 0) = 1.0 / (rhos_[i]+1);
        }
        if (pAnnRho_) delete pAnnRho_;
        //pAnnRho_ = new FLAnnWarper < MATH::L2_Distance_Inverse > ;
        pAnnRho_ = new FLAnnWarper < MATH::L2_Distance >(1);
        pAnnRho_->init(inverseRho);
        pAnnRho_->setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));

        deltas_.resize(nSize, 0);

        std::vector<int> nnIdx;
        std::vector<double> nnDists;
        for (size_t i = 0; i < nSize; i++)
        {
            nnIdx.clear();
            nnDists.clear();
            if (i%20==0)
                std::cout << " computing..\r" << i*100.0/nSize << "%";

            Eigen::VectorXd query(1);
            query(0) = 0.0;
            double radius = 1.0 / (rhos_[i] + 1);
            pAnnRho_->radiusSearch(query, nnIdx, nnDists, radius*radius);

            double minDist = DBL_MAX;
            int minIdx = -1;
            Eigen::VectorXd pi = data_.row(i);
            for (size_t j = 0; j < nnIdx.size(); j++)
            {
                if (nnIdx[j] == i || rhos_[nnIdx[j]] <= rhos_[i])
                    continue;

                Eigen::VectorXd pj = data_.row(nnIdx[j]);
                double distance = distance_l2(pi, pj);
                if (distance < minDist)
                {
                    minDist = distance;
                    minIdx = nnIdx[j];
                }
            }
            if (minIdx < 0)
            {
                deltas_[i] = param_.maxDist;
            }
            else
            {
                deltas_[i] = minDist;
            }
        }
        std::cout << std::endl;

    }

    void DensityPeak::computeTimes()
    {
        int nSize = getPointSize();
        std::vector<double> rhoNorm = MathTools::normalize(rhos_);
        std::vector<double> deltaNorm = MathTools::normalize(deltas_);
        rhoTimesDeltas_.resize(nSize, 0);
        for (size_t i = 0; i < nSize; i++)
        {
            rhoTimesDeltas_[i] = sqrt(rhoNorm[i] * deltaNorm[i]);
        }
    }

    bool DensityPeak::analyseResult()
    {
        int nSize = getPointSize();
        if (nSize <= 0)
            return false;

        std::vector<double> rhoN = MathTools::normalize(rhos_);
        std::vector<double> delN = MathTools::normalize(deltas_);
        std::vector<SortData> toSort(nSize);

        for (size_t i = 0; i < nSize; i++)
        {
            toSort[i].data = rhoN[i] * delN[i];
            toSort[i].index = i;
        }
        std::sort(toSort.begin(), toSort.end(), compare);

        double sumGap = 0;
        int count = 0;
        std::vector<int> clusterCenterIds;
        std::vector<SortData>::iterator it = toSort.begin();
        std::vector<SortData>::iterator itPrev = it;
        it++;
        clusterCenterIds.push_back(itPrev->index);
        while (it != toSort.end())
        {
            double curGap = itPrev->data - it->data;
            it++;
            itPrev++;
            count++;
            if (count >= 5 && curGap > sumGap / count*param_.thresholdGap)
                break;
            
            sumGap += curGap;
            clusterCenterIds.push_back(itPrev->index);
        }

        DebugLog << " Cluster center size: " << clusterCenterIds.size();
        clusterCenterIds_ = clusterCenterIds;

        return true;
    }

    bool DensityPeak::clusterAll()
    {
        if (data_.cols()==0 || getPointSize()==0)
            return false;

        if (clusterCenterIds_.size()==0)
            return false;

        int nSize = getPointSize();
        int nDim = getPointDim();
        int nCenterCount = clusterCenterIds_.size();
        clusterIds_.resize(nSize, -1);

        // build ann data structure for this search
        FLAnnWarper<MATH::L2_Distance> ann(1);
        Eigen::MatrixXd centerData(nCenterCount, nDim);
        std::vector< std::vector<int> > nnIdx;
        std::vector< std::vector<double> > nnDists;
        for (int i = 0; i < nCenterCount; i++)
        {
            for (int j = 0; j < nDim; j++)
            {
                centerData(i, j) = data_(clusterCenterIds_[i], j);
            }
        }
//        DefaultLog << centerData;
        ann.init(centerData);
        ann.setSearchParams(flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
        //std::vector<int> idx;
        //std::vector<double> dists;
        //ann.knnSearch(data_.row(0), idx, dists, 2);
        ann.knnSearch(data_, nnIdx, nnDists, 2);
        for (int i = 0; i < nSize; i++)
        {
            if (nnDists[i][0]<param_.dc)
                clusterIds_[i] = nnIdx[i][0];
            else
                clusterIds_[i] = -1;
        }
        return true;
    }
}

GLOBAL_NAMESPACE_END